import matrix
import math
import vectorfield
import random
#random.seed(2)

ROBOT_ARM_L1 = 4
ROBOT_ARM_L2 = 3
ROBOT_G = 2
ROBOT_MOUNT = (0,5,0)
ROBOT_ARMLIMITS = (-2.9671, 2.9671) # ~ 170 degrees each direction
ROBOT_SPINLIMITS = (0, 2*math.pi)
ROBOT_GRIPLIMITS = (0.2, 4.5)

def sq_dist(p1, p2):
	return sum([(v1-v2)**2 for v1,v2 in zip(p1,p2)])

def grip_center(j1, j2, j3):
		# second arm section to grip center
		l2 = ROBOT_ARM_L2 + ROBOT_G
		# first arm section
		l1 = ROBOT_ARM_L1
		
		# find point location after rotation
		p = matrix.Matrix(3,1)
		p.set(0,0,0)
		p.set(1,0,l2)
		p.set(2,0,0)
		j3Mat = matrix.Matrix(3,3)
		j3Mat.make_rot_z(j3)
		p = j3Mat.mul(p)
		p.set(1,0,p.get(1,0)+l1)
		j2Mat = matrix.Matrix(3,3)
		j2Mat.make_rot_z(j2)
		p = j2Mat.mul(p)
		j1Mat = matrix.Matrix(3,3)
		j1Mat.make_rot_y(j1)
		p = j1Mat.mul(p)
		p = [p.get(i,0) for i in range(3)]
		p[1] += ROBOT_MOUNT[1]
		return tuple(p)

def grip_pos(q):
	return grip_center(q[0], q[1], q[2])
						
def gripper_pos_equality(q, gripObjPos, printFlag=False):
	g = grip_pos(q)
	res = [g[i] - p for i,p in enumerate(gripObjPos)]
	if printFlag:
		print "Equality test:", res
	return res
	
def gripper_width_equality(q, gripObjWidth):
	return q[4] - gripObjWidth

def gripper_width_jacobian(q, gripObjWidth):
	J = [0.0]*len(q)
	J[4] = 1.0
	return J

def gripper_height_diff(q, gripObjWidth):
	return q[1] - (gripObjWidth / 2.0)

def gripper_target_diff(q):
	global targetPt
	return [q[0] - targetPt[0], q[2] - targetPt[1]]

def collision_free(q,balls,graspedObj, ballRadii):
	for i,b in enumerate(balls):
		if i == graspedObj: continue
		#TODO: test robot geometry against ball b
	return True

def collision_free_ball(balls, index, position, ballRadii):
	for i,ball1 in enumerate(balls):
		if index == i: continue
		
		d = sq_dist(ball1, position)
		if d <  (ballRadii[i] + ballRadii[index])**2:
			return False
	return True
